import cv2
import numpy as np
import matplotlib.pyplot as plt
from PIL import Image

'''
输入左右相机的理想内参数：
fx、fy：镜头焦距8mm，像元尺寸5.3微米，内参中的fx、fy的理想值为8/0.0053；
u0、v0：图像尺寸为1280*1024，u0和v0的理想数值为640和512。
'''
mtx_l = np.array([[100 / 0.0053, 0, 640, 0],
                  [0, 100 / 0.0053, 512, 0],
                  [0, 0, 1, 0]])
mtx_r = np.array([[100 / 0.0053, 0, 640, 0],
                  [0, 100 / 0.0053, 512, 0],
                  [0, 0, 1, 0]])

'''
双目相机理想外参数：
旋转矩阵：左相机坐标系到右相机坐标系的旋转矩阵；
平移矩阵：两个相机之间的距离。
'''
R_lr = np.array([[1, 0, 0],
                 [0, 1, 0],
                 [0, 0, 1]])
T_lr = np.array([[800],
                 [0],
                 [0]])

# 左相机本身的旋转矩阵和平移矩阵
R_ll = ([[1, 0, 0],
         [0, 1, 0],
         [0, 0, 1]])
T_ll = ([[0], [0], [0]])

# 左相机到左相机的投影矩阵
temp_R_ll = np.append(R_ll, T_ll, axis=1)
_temp_R_ll = np.row_stack((temp_R_ll, [0, 0, 0, 1]))
Pl = np.dot(mtx_l, _temp_R_ll)

# 右相机到左相机的投影矩阵
temp_R_lr = np.append(R_lr, T_lr, axis=1)
_temp_R_lr = np.row_stack((temp_R_lr, [0, 0, 0, 1]))
Pr = np.dot(mtx_r, _temp_R_lr)

'''
像素坐标系下空间点坐标：
以成像面左上角为坐标原点建立像素坐标系，测量空间投影点在像素坐标系下的坐标，坐标值除以像元大小，得到像素坐标。
本仿真共测量了三个空间点p、q、m。
'''
lp = np.array([1.38201300/0.0053, 2.71360000/0.0053])
rp = np.array([5.40198700/0.0053, 2.71360000/0.0053])

'''
triangulatePoints()函数计算空间点三维坐标：
输入：以左相机为参考，输入左右相机的投影矩阵，以及左右相机采集到的空间点质心像素坐标；
输出：相机坐标系下空间点三维坐标的归一化矩阵，若得到三维坐标，需要将矩阵中前三个元素分别除以矩阵中的第四个元素。
'''
P = np.array(cv2.triangulatePoints(Pl, Pr, lp, rp))

P /= P[3]

# 输出空间点三维坐标
print(P)

